#include "app_porting.h"
#include "mpu9250.h"

//#define INVENSENSE_TASK_DEBUG_ENABLE
//#define INVENSENSE_TIME_MEASURE

#define INVENSENSE_IMPORT_PRINT

#define DEG2RAD		0.017453293f	/* 度转弧度 π/180 */
#define RAD2DEG		57.29578f		/* 弧度转度 180/π */

uint8_t real_deep;
#if 1
uint16_t real_radius=587;//mm	
#else
uint16_t real_radius=916;//mm	
#endif

static OS_EVENT *sg_sem_invensense=NULL;
void sem_invensense_init(void)
{
	sg_sem_invensense=OSSemCreate(0);
	if(NULL==sg_sem_invensense){
		//
	}	
}

void sem_invensense_post(void)
{
	if(NULL!=sg_sem_invensense){
		OSSemPost(sg_sem_invensense);	
	}else{
		//
	}
}

void sem_invensense_pend(void)
{
	INT8U err;
	if(NULL!=sg_sem_invensense){
		OSSemPend(sg_sem_invensense,0,&err);
	}else{
		//
	}
}

static OS_EVENT *sg_sem_deep;
void sem_deep_init(void)
{
	sg_sem_deep=OSSemCreate(1);
	if(NULL==sg_sem_deep){
	}	
}

void sem_deep_post(void)
{
	if(NULL!=sg_sem_deep){
		OSSemPost(sg_sem_deep);
	}else{
	}
}

void sem_deep_pend(void)
{
	INT8U err;
	
	if(NULL!=sg_sem_deep){
		OSSemPend(sg_sem_deep,0,&err);
	}else{
	}	
}

#ifdef INVENSENSE_TASK_DEBUG_ENABLE
void invensense_printf(INVENSENSE_RAW_DATA_T *pcontent)
{	

	printf("=======================================\r\n");
	printf("quaternion_accuracy=%d\r\n",pcontent->quaternion_accuracy);	
	printf("quaternion_timestamp=%d\r\n",pcontent->quaternion_timestamp);
	printf("quaternion[%d,%d,%d,%d]\r\n",pcontent->quaternion[0],pcontent->quaternion[1]
										,pcontent->quaternion[2],pcontent->quaternion[3]);
	printf("euler_accuracy=%d\r\n",pcontent->euler_accuracy);	
	printf("euler_timestamp=%d\r\n",pcontent->euler_timestamp);
	printf("euler[%d,%d,%d]\r\n",pcontent->euler[0],pcontent->euler[1]
										,pcontent->euler[2]);	
	printf("accel_accuracy=%d\r\n",pcontent->accel_accuracy);	
	printf("accel_timestamp=%d\r\n",pcontent->accel_timestamp);
	printf("accel[%d,%d,%d]\r\n",pcontent->accel[0],pcontent->accel[1]
										,pcontent->accel[2]);	
	
	printf("gyro_accuracy=%d\r\n",pcontent->gyro_accuracy);	
	printf("gyro_timestamp=%d\r\n",pcontent->gyro_timestamp);
	printf("gyro[%d,%d,%d]\r\n",pcontent->gyro[0],pcontent->gyro[1]
										,pcontent->gyro[2]);	
	printf("compass_accuracy=%d\r\n",pcontent->compass_accuracy);	
	printf("compass=%d\r\n",pcontent->compass_timestamp);
	printf("compass[%d,%d,%d]\r\n",pcontent->compass[0],pcontent->compass[1]
										,pcontent->compass[2]);	
	printf("=======================================\r\n");	
}
#endif

static long sg_current_angle;
void current_angle_set(long angle)
{
	sem_deep_pend();
	sg_current_angle=angle;
	sem_deep_post();
}

void current_angle_get(long *pangle)
{
	sem_deep_pend();
	*pangle = sg_current_angle;
	sem_deep_post();
}

static long sg_cilibrate_angle;
void clibrate_angle_set(long angle)
{
	sem_deep_pend();
	sg_cilibrate_angle=angle;
	sem_deep_post();
}

void clibrate_angle_get(long *pangle)
{
	sem_deep_pend();
	*pangle = sg_cilibrate_angle;
	sem_deep_post();
}

uint8_t deep_calculate(long current_angle)
{
	uint8_t depth=0;
	long clibrate_angle;
	float current_sin;
	float clibrate_sin;
	float radius;

	clibrate_angle_get(&clibrate_angle);
	radius=(float)real_radius;


	if(current_angle<clibrate_angle){
#ifdef INVENSENSE_IMPORT_PRINT
		printf("[real]depth=%d,current_angle=%d,clibrate_angle=%d,radius=%d\r\n",depth,current_angle,clibrate_angle,real_radius);
#endif
		return 0;
	}

	current_sin=(float)current_angle;
	current_sin/=65536.f;
	current_sin*=DEG2RAD;
	current_sin=sin(current_sin);

	clibrate_sin=(float)clibrate_angle;
	clibrate_sin/=65536.f;
	clibrate_sin*=DEG2RAD;
	clibrate_sin=sin(clibrate_sin);

	
	radius/=10;//mm->cm
	depth=(uint8_t)((radius*(current_sin-clibrate_sin))+0.5f);
#ifdef INVENSENSE_IMPORT_PRINT
	printf("[real]depth=%d,current_angle=%d,clibrate_angle=%d,radius=%d\r\n",depth,current_angle,clibrate_angle,real_radius);
#endif
	
	return depth;
}

void real_calibrate(void)
{
	long angle;

	current_angle_get(&angle);		
	printf("[%s](%d)angle=%d\r\n",__FUNCTION__,__LINE__,angle);
	g_product_info.content.initial_angle=angle;
	product_info_write(&g_product_info);
	clibrate_angle_set(angle);	
}


void invensense_task_init(void)
{
	invensense_hw_init();	
	sem_invensense_init();
	sem_deep_init();
	clibrate_angle_set(g_product_info.content.initial_angle);
}

/*不进行过滤*/
//#define DEPTH_FILTER_MATHOD_NONE
/*相邻的两个采集深度不相等过滤掉*/
//#define DEPTH_FILTER_MATHOD_FIRST
/*相邻的两个采集深度相差大于2过滤掉*/
//#define DEPTH_FILTER_MATHOD_SENCOND
/*1.5秒30个采样点,10采样点算一次深度,三次采样的数值相等才更新,或者三次采样的差值<3才更新*/
//#define DEPTH_FILTER_MATHOD_THIRD
/*kalman滤波*/
//#define DEPTH_FILTER_MATHOD_KALMAN
/*1.5秒30个采样点,10采样点算一次深度,三次采样的数值相等才更新,或者三次采样的差值<3才更新
+kalman滤波*/
#define DEPTH_FILTER_MATHOD_KALMAN_SECOND


#ifdef DEPTH_FILTER_MATHOD_NONE

#define INVENSENSE_FILTER_LEN 30
#define INVENSENSE_USE_LEN 6
void invensense_task(void *pdata)
{	
	INVENSENSE_RAW_DATA_T dat;
#ifdef INVENSENSE_TIME_MEASURE
	uint16_t orig_time;
#endif
	int angle_cnt;	
	long angle;
	long angle_array[INVENSENSE_FILTER_LEN];
	int i,j;
	
	
	pdata=pdata;
#ifdef LIB_PRINTF_SUPPORT
	printf("[%s](%d) task created.\r\n",__FUNCTION__,__LINE__);
#endif		
	angle_cnt=0;
	
	while(1)
	{	
#ifdef INVENSENSE_TIME_MEASURE
		time_measure_start(&orig_time); 
#endif
		sem_invensense_pend();
		if(LS_SUCCESS==motion_read(&dat)){
#ifdef INVENSENSE_TASK_DEBUG_ENABLE
		invensense_printf(&dat);
#endif
#ifdef INVENSENSE_TIME_MEASURE
			printf("ytis_receive_times=%d(ms).\r\n",time_measure_get_ms(orig_time));	
#endif
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
			angle=dat.euler[0];
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				angle_array[angle_cnt++]=angle;
			}else{
				for(i=0;i<INVENSENSE_FILTER_LEN-1;i++){
					angle_array[i]=angle_array[i+1];					
				}
				angle_array[i]=angle;
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				continue;
			}
			
		//sort
			for(i=0;i<angle_cnt;i++){
				for(j=0;j<angle_cnt-i-1;j++){
					if(angle_array[j]>angle_array[j+1]){
						angle=angle_array[j+1];
						angle_array[j+1]=angle_array[j];
						angle_array[j]=angle;
					}
				}
			}
			
			angle=0;
			for(i=(INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2;i<(angle_cnt-((INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2));i++){
				angle+=angle_array[i];
			}
			angle/=INVENSENSE_USE_LEN;
			angle_cnt=0;

			current_angle_set(angle);
			real_deep=deep_calculate(angle);
		}else{
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
		}
	}
}

#elif defined(DEPTH_FILTER_MATHOD_FIRST)

#define INVENSENSE_FILTER_LEN 30
#define INVENSENSE_USE_LEN 6
void invensense_task(void *pdata)
{	
	INVENSENSE_RAW_DATA_T dat;
#ifdef INVENSENSE_TIME_MEASURE
	uint16_t orig_time;
#endif
	int angle_cnt;	
	long angle;
	long angle_array[INVENSENSE_FILTER_LEN];
	int i,j;
	uint8_t curr_depth=0;
	uint8_t back_depth=0;
	
	pdata=pdata;
#ifdef LIB_PRINTF_SUPPORT
	printf("[%s](%d) task created.\r\n",__FUNCTION__,__LINE__);
#endif		
	angle_cnt=0;
	
	while(1)
	{	
#ifdef INVENSENSE_TIME_MEASURE
		time_measure_start(&orig_time); 
#endif
		sem_invensense_pend();
		if(LS_SUCCESS==motion_read(&dat)){
#ifdef INVENSENSE_TASK_DEBUG_ENABLE
		invensense_printf(&dat);
#endif
#ifdef INVENSENSE_TIME_MEASURE
			printf("ytis_receive_times=%d(ms).\r\n",time_measure_get_ms(orig_time));	
#endif
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
			angle=dat.euler[0];
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				angle_array[angle_cnt++]=angle;
			}else{
				for(i=0;i<INVENSENSE_FILTER_LEN-1;i++){
					angle_array[i]=angle_array[i+1];					
				}
				angle_array[i]=angle;
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				continue;
			}
			
		//sort
			for(i=0;i<angle_cnt;i++){
				for(j=0;j<angle_cnt-i-1;j++){
					if(angle_array[j]>angle_array[j+1]){
						angle=angle_array[j+1];
						angle_array[j+1]=angle_array[j];
						angle_array[j]=angle;
					}
				}
			}
			
			angle=0;
			for(i=(INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2;i<(angle_cnt-((INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2));i++){
				angle+=angle_array[i];
			}
			angle/=INVENSENSE_USE_LEN;
			angle_cnt=0;

			current_angle_set(angle);
			curr_depth=deep_calculate(angle);
			if(curr_depth==back_depth){
				real_deep=curr_depth;
			}
			back_depth=curr_depth;
		}else{
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
		}
	}
}

#elif defined(DEPTH_FILTER_MATHOD_SENCOND)

#define INVENSENSE_FILTER_LEN 30
#define INVENSENSE_USE_LEN 6
#define ANGLE_THRESHOLD (2.5*65536)

void invensense_task(void *pdata)
{	
	INVENSENSE_RAW_DATA_T dat;
#ifdef INVENSENSE_TIME_MEASURE
	uint16_t orig_time;
#endif
	int angle_cnt;	
	long angle;
	long angle_bak;
	long angle_array[INVENSENSE_FILTER_LEN];
	int i,j;
	
	pdata=pdata;
#ifdef LIB_PRINTF_SUPPORT
	printf("[%s](%d) task created.\r\n",__FUNCTION__,__LINE__);
#endif		
	angle_cnt=0;
	angle_bak=0;
	while(1)
	{	
#ifdef INVENSENSE_TIME_MEASURE
		time_measure_start(&orig_time); 
#endif
		sem_invensense_pend();
		if(LS_SUCCESS==motion_read(&dat)){
#ifdef INVENSENSE_TASK_DEBUG_ENABLE
		invensense_printf(&dat);
#endif
#ifdef INVENSENSE_TIME_MEASURE
			printf("ytis_receive_times=%d(ms).\r\n",time_measure_get_ms(orig_time));	
#endif
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
			angle=dat.euler[0];
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				angle_array[angle_cnt++]=angle;
			}else{
				for(i=0;i<INVENSENSE_FILTER_LEN-1;i++){
					angle_array[i]=angle_array[i+1];					
				}
				angle_array[i]=angle;
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				continue;
			}
			
		//sort
			for(i=0;i<angle_cnt;i++){
				for(j=0;j<angle_cnt-i-1;j++){
					if(angle_array[j]>angle_array[j+1]){
						angle=angle_array[j+1];
						angle_array[j+1]=angle_array[j];
						angle_array[j]=angle;
					}
				}
			}
			
			angle=0;
			for(i=(INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2;i<(angle_cnt-((INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2));i++){
				angle+=angle_array[i];
			}
			angle/=INVENSENSE_USE_LEN;
			angle_cnt=0;

			if(angle>angle_bak){
				if((angle-angle_bak)<ANGLE_THRESHOLD){					
					real_deep=deep_calculate(angle);					
				}
			}else{
				if((angle_bak-angle)<ANGLE_THRESHOLD){
					real_deep=deep_calculate(angle);					
				}
			}
			angle_bak=angle;
			current_angle_set(angle);			
		}else{
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
		}
	}
}

#elif defined(DEPTH_FILTER_MATHOD_THIRD)

#define INVENSENSE_FILTER_LEN 10
#define INVENSENSE_USE_LEN 4
#define ANGLE_THRESHOLD (1.5*65536)
void invensense_task(void *pdata)
{	
#define SAMPLES_CNT_MAX 3

	INVENSENSE_RAW_DATA_T dat;
#ifdef INVENSENSE_TIME_MEASURE
	uint16_t orig_time;
#endif
	int angle_cnt;	
	long angle;
	long angle_array[INVENSENSE_FILTER_LEN];
	int i,j;

	long samples[SAMPLES_CNT_MAX];
	uint8_t samples_index;
	long sample_temp;
	long samples_total;
	
	pdata=pdata;
#ifdef LIB_PRINTF_SUPPORT
	printf("[%s](%d) task created.\r\n",__FUNCTION__,__LINE__);
#endif		
	angle_cnt=0;
	samples_index=0;
	
	while(1)
	{	
#ifdef INVENSENSE_TIME_MEASURE
		time_measure_start(&orig_time); 
#endif
		sem_invensense_pend();
		if(LS_SUCCESS==motion_read(&dat)){
#ifdef INVENSENSE_TASK_DEBUG_ENABLE
		invensense_printf(&dat);
#endif
#ifdef INVENSENSE_TIME_MEASURE
			printf("ytis_receive_times=%d(ms).\r\n",time_measure_get_ms(orig_time));	
#endif
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
			angle=dat.euler[0];
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				angle_array[angle_cnt++]=angle;
			}else{
				for(i=0;i<INVENSENSE_FILTER_LEN-1;i++){
					angle_array[i]=angle_array[i+1];					
				}
				angle_array[i]=angle;
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				continue;
			}
			
		//sort
			for(i=0;i<angle_cnt;i++){
				for(j=0;j<angle_cnt-i-1;j++){
					if(angle_array[j]>angle_array[j+1]){
						angle=angle_array[j+1];
						angle_array[j+1]=angle_array[j];
						angle_array[j]=angle;
					}
				}
			}
			
			angle=0;
			for(i=(INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2;i<(angle_cnt-((INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2));i++){
				angle+=angle_array[i];
			}
			angle/=INVENSENSE_USE_LEN;
			angle_cnt=0;

			samples[samples_index++]=angle;
			if(samples_index>=SAMPLES_CNT_MAX){
				samples_index=0;
				for(i=0;i<SAMPLES_CNT_MAX;i++){
					for(j=0;j<SAMPLES_CNT_MAX-i-1;j++){
						if(samples[j]>samples[j+1]){
							sample_temp=samples[j+1];
							samples[j+1]=samples[j];
							samples[j]=sample_temp;
						}
					}
				}
				
				if((samples[SAMPLES_CNT_MAX-1]-samples[0])<=ANGLE_THRESHOLD){
					samples_total=0;
					for(i=0;i<SAMPLES_CNT_MAX;i++){
						samples_total+=samples[i];
					}
					samples_total/=SAMPLES_CNT_MAX;
					current_angle_set(samples_total);
					real_deep=(uint8_t)deep_calculate(samples_total);
				}
			}
			
		}else{
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
		}
	}
}

#elif defined(DEPTH_FILTER_MATHOD_KALMAN)

#define INVENSENSE_FILTER_LEN 30
#define INVENSENSE_USE_LEN 6
void invensense_task(void *pdata)
{	
	INVENSENSE_RAW_DATA_T dat;
#ifdef INVENSENSE_TIME_MEASURE
	uint16_t orig_time;
#endif
	int angle_cnt;	
	long angle;
	long angle_array[INVENSENSE_FILTER_LEN];
	int i,j;

	uint8_t kalman_init_flag=0;
	
	pdata=pdata;
#ifdef LIB_PRINTF_SUPPORT
	printf("[%s](%d) task created.\r\n",__FUNCTION__,__LINE__);
#endif		
	angle_cnt=0;
	
	while(1)
	{	
#ifdef INVENSENSE_TIME_MEASURE
		time_measure_start(&orig_time); 
#endif
		sem_invensense_pend();
		if(LS_SUCCESS==motion_read(&dat)){
#ifdef INVENSENSE_TASK_DEBUG_ENABLE
		invensense_printf(&dat);
#endif
#ifdef INVENSENSE_TIME_MEASURE
			printf("ytis_receive_times=%d(ms).\r\n",time_measure_get_ms(orig_time));	
#endif
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
			angle=dat.euler[0];
			if(kalman_init_flag){
				angle=kalman_fillter(angle);
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				angle_array[angle_cnt++]=angle;
			}else{
				for(i=0;i<INVENSENSE_FILTER_LEN-1;i++){
					angle_array[i]=angle_array[i+1];					
				}
				angle_array[i]=angle;
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				continue;
			}
			
		//sort
			for(i=0;i<angle_cnt;i++){
				for(j=0;j<angle_cnt-i-1;j++){
					if(angle_array[j]>angle_array[j+1]){
						angle=angle_array[j+1];
						angle_array[j+1]=angle_array[j];
						angle_array[j]=angle;
					}
				}
			}
			
			angle=0;
			for(i=(INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2;i<(angle_cnt-((INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2));i++){
				angle+=angle_array[i];
			}
			angle/=INVENSENSE_USE_LEN;
			angle_cnt=0;

			current_angle_set(angle);
			real_deep=deep_calculate(angle);
			if(0==kalman_init_flag)
			{
				kalman_init_flag=1;
				kalman_init(angle);
			}
		}else{
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
		}
	}
}

#elif defined(DEPTH_FILTER_MATHOD_KALMAN_SECOND)

#define INVENSENSE_FILTER_LEN 10
#define INVENSENSE_USE_LEN 4
#define ANGLE_THRESHOLD (1.2*65536)
void invensense_task(void *pdata)
{	
#define SAMPLES_CNT_MAX 3

	INVENSENSE_RAW_DATA_T dat;
#ifdef INVENSENSE_TIME_MEASURE
	uint16_t orig_time;
#endif
	int angle_cnt;	
	long angle;
	long angle_array[INVENSENSE_FILTER_LEN];
	int i,j;

	long samples[SAMPLES_CNT_MAX];
	uint8_t samples_index;
	long sample_temp;
	long samples_total;
	uint8_t kalman_init_flag=0;
	
	pdata=pdata;
#ifdef LIB_PRINTF_SUPPORT
	printf("[%s](%d) task created.\r\n",__FUNCTION__,__LINE__);
#endif		
	angle_cnt=0;
	samples_index=0;
	
	while(1)
	{	
#ifdef INVENSENSE_TIME_MEASURE
		time_measure_start(&orig_time); 
#endif
		sem_invensense_pend();
		if(LS_SUCCESS==motion_read(&dat)){
#ifdef INVENSENSE_TASK_DEBUG_ENABLE
		invensense_printf(&dat);
#endif
#ifdef INVENSENSE_TIME_MEASURE
			printf("ytis_receive_times=%d(ms).\r\n",time_measure_get_ms(orig_time));	
#endif
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
			angle=dat.euler[0];
			if(kalman_init_flag){
				angle=kalman_fillter(angle);
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				angle_array[angle_cnt++]=angle;
			}else{
				for(i=0;i<INVENSENSE_FILTER_LEN-1;i++){
					angle_array[i]=angle_array[i+1];					
				}
				angle_array[i]=angle;
			}
			
			if(angle_cnt<INVENSENSE_FILTER_LEN){
				continue;
			}
			
		//sort
			for(i=0;i<angle_cnt;i++){
				for(j=0;j<angle_cnt-i-1;j++){
					if(angle_array[j]>angle_array[j+1]){
						angle=angle_array[j+1];
						angle_array[j+1]=angle_array[j];
						angle_array[j]=angle;
					}
				}
			}
			
			angle=0;
			for(i=(INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2;i<(angle_cnt-((INVENSENSE_FILTER_LEN-INVENSENSE_USE_LEN)/2));i++){
				angle+=angle_array[i];
			}
			angle/=INVENSENSE_USE_LEN;
			angle_cnt=0;

			samples[samples_index++]=angle;
			if(samples_index>=SAMPLES_CNT_MAX){
				samples_index=0;
				for(i=0;i<SAMPLES_CNT_MAX;i++){
					for(j=0;j<SAMPLES_CNT_MAX-i-1;j++){
						if(samples[j]>samples[j+1]){
							sample_temp=samples[j+1];
							samples[j+1]=samples[j];
							samples[j]=sample_temp;
						}
					}
				}
				
				if((samples[SAMPLES_CNT_MAX-1]-samples[0])<=ANGLE_THRESHOLD){
					samples_total=0;
					for(i=0;i<SAMPLES_CNT_MAX;i++){
						samples_total+=samples[i];
					}
					samples_total/=SAMPLES_CNT_MAX;
					current_angle_set(samples_total);
					real_deep=(uint8_t)deep_calculate(samples_total);
					if(0==kalman_init_flag){
						kalman_init_flag=1;
						kalman_init(samples_total);
					}
				}
			}
			
		}else{
#if 0//def INVENSENSE_TASK_DEBUG_ENABLE
			printf("[%s](%d)\r\n",__FUNCTION__,__LINE__);
#endif
		}
	}
}

#endif

